/*  
 *  Nanjing JIYU Technology Co.,Ltd All rights reserved.
 *  Copyright (C) 2022 
 *  ==============================================================
 * 
 *  automatic.c 
 *    自动运行模式下运行逻辑的实现
 *
 *  --------------------------------------------------------------
 *  
 */

#include "automatic.h"

#include "status.h"
#include "track.h"

#include "device.h"


static void auto_0_1(void)
{
	/* 防止起始框干扰 */
	track_drive(speed_medium);
	delay_ms(2000);
	
  /* 小车运行 */
  while (1)
  {
    delay_ms(25);
    track_drive(speed_medium);

    /* 遇到1号岔路口，停车跳出循环 */
    if (gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }

  status.location = '1';    /* 到达位置1 */
}


static void auto_1_2(void)
{
  /* 往前开一点，防止当前路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到2号岔路口，停车跳出循环 */
    if (gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }

  status.location = '2';    /* 到达位置2 */
}

/* 3为直角右转 */
static void auto_2_3(void)
{
  /* 往前开一点，防止当前路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到3号转弯，停车跳出循环 */
    if (gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }
  status.location = '3';    /* 到达位置3 */
}

static void auto_x_4(void)
{
	/* 123号位到4，都需要先转向 */
	track_turn_right(speed_medium);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到需要转弯的路口，停车，跳出控制循环 */
    if (gray_is_left_corner())
    {
      track_stop();
      break;
    }
  }

  status.location = '4';    /* 到达位置4 */
}

static void auto_4_5(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_turn_left(speed_medium);

  /* 前进控制 */
  while (1)
  {
    delay_ms(10);
    track_drive(speed_medium);

    /* 遇到停车框的T字路口，停车，跳出控制循环 */
    if (gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }

  status.location = '5';    /* 到达位置5 */
}


/* 在5号点，需要把物块放到传送带上 */
static void auto_5(void)
{
	
  /* 慢速前进到停止线 */
  while (1)
  {
    delay_ms(10);
    track_drive(speed_low);

    /* 遇到前停止线，停车，跳出控制循环 */
    if (gray_front_stop_line())
    {
      
			track_stop();
      break;
    }
  }
	
	servo_put_down();		/* 放下物块 */
	delay_ms(1000);
	buzzer_tip();
  /* 慢速退到5号点 */	
  while (1)
  {
    delay_ms(10);
    track_reverse(speed_low);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }
	
	servo_take_up(); /* 合上抓手 */
	delay_ms(1000);
	
}


static void auto_5_6(void)
{
	track_turn_right(speed_medium);
	
  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium); 

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_left_corner() || gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }

  status.location = '6';    /* 到达位置6 */
}

/* 在6号点，需要把物块从传送带上拿起 */
static void auto_6(void)
{
	servo_put_down();		/* 打开夹手 */
	delay_ms(1000);
	
	track_turn_left_time(speed_medium, 800);
//	track_turn_left(speed_medium);
	
	delay_ms(1000);
	servo_take_up();		/* 夹取物块 */
	delay_ms(1000);
	
	track_turn_right(speed_medium);
}

static void auto_6_7(void)
{
	track_drive(speed_medium);
	delay_ms(500);
	
  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_left_corner() || gray_is_right_corner())
    {
      track_stop();
      break;
    }
  }
  status.location = '7';    /* 到达位置7 */
}

static void auto_7_8(void)
{
	track_turn_left(speed_medium);
	
  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到投放点的T字路口，停车，跳出控制循环 */
    if (gray_is_left_corner() || gray_is_right_corner())
    {
			track_stop();
      break;
    }
  }
  status.location = '8';    /* 到达位置8 */
}

/* 在8号点，需要把物块放在抛投框上 */
static void auto_8(void)
{
	servo_put_down();
	
	delay_ms(300);
	
	track_reverse(speed_medium);
	delay_ms(1000);
	
	track_stop();
	
	servo_take_up();
}

static void auto_8_0(void)
{
	track_reverse(speed_medium);
	delay_ms(500);
	
  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_reverse(speed_medium);

    /* 遇到启止区的框，倒着走，所以是右边 */
    if (gray_is_right_corner())
    {
      break;
    }
  }
	
	delay_ms(1000);
	
	track_stop();
	
  status.location = '0';    /* 到达位置8 */

}



/* 控制函数表 */
static void auto_drive_fun_lst(unsigned char destination)
{
  switch (destination)
  {
    case '1' :
      auto_0_1();
      break;
    case '2' :
      auto_1_2();
      break;
    case '3' :
      auto_2_3();
      break;
    case '4' :
      auto_x_4();
      break;
    case '5' :
      auto_4_5();
      break;
    case '6' :
      auto_5_6();
      break;
    case '7' :
      auto_6_7();
      break;
    case '8' :
      auto_7_8();
      break;
    case '9' :
      auto_8_0();
      break;		
    default  :
      break;
  }
}

void auto_run(unsigned char location, unsigned char destination)
{
	for (int i = location + 1; i <= destination; i++)
	{
		auto_drive_fun_lst(i);
	}
}

void auto_transporter_start(void)
{
  auto_5();
}

void auto_transporter_end(void)
{
  auto_6();
}

void auto_throw(void)
{
  auto_8();
}

/* 前进一步测试 */
void auto_ctrl_test(void)
{
  while (1)
  {
    if (status.location == status.destination - 1)
    {
      switch (status.destination)
      {
        case '0' :
          status.destination = 0;
          break;
        case '1' :
          auto_0_1();
          break;
        case '2' :
          auto_1_2();
          break;
        case '3' :
          auto_2_3();
          break;
        case '4' :
          auto_x_4();
          break;
        case '5' :
          auto_4_5();
          break;
        case '6' :
          auto_5_6();
          break;
        case '7' :
          auto_6_7();
          break;
        case '8' :
          auto_7_8();
          break;
        default  :
          break;
      }
    }
  }
}

void auto_run_test(void)
{
//	auto_5();
//	auto_5_6();
//	auto_6();
//	auto_8();
	
	auto_0_1();
	auto_1_2();
	auto_2_3();
	auto_x_4();
	auto_4_5();
	auto_5_6();
	auto_6_7();
	auto_7_8();
	auto_8();
	
	auto_8_0();
	
	while(1);
}


